# -*- coding: utf-8 -*-
"""
Created on Thu Jun 07 10:25:47 2012
Bezier curves
@author: Maxim
"""
import numpy as ny
import matplotlib.pyplot as plt
import sys
import scipy.interpolate as interp
import math
from scipy.optimize import bisect, newton
from scipy.optimize import fminbound

def factorial(n):
    n = ny.abs(int(n)) 
    if n < 1: n = 1 
    if n == 1: 
        return 1 
    else: 
        return n * factorial(n - 1)

def BPOcoef(order):
    K = ny.ones([order+1,1],int)
    for ii in range(1,order):
        K[ii] = factorial(order)/(factorial(ii)*factorial(order-ii))
    return K

def BezierCurve(Pts, opt):
    order, dimm = Pts.shape
    order -=1
    t = 0
    K = BPOcoef(order)
    if opt>1:
        Npts = int(opt)
        Curve = ny.zeros([Npts,dimm+1])
        Step = 1./(Npts-1)
        
        for ii in range(Npts):
            Curve[ii,-1] = t
            for jj in range(dimm):
                for kk in range(order+1):
                    Curve[ii,jj] += K[kk]*Pts[kk,jj]*(1-t)**(order-kk)*t**kk
            t +=Step
    elif opt>=0 and opt<=1:
        Curve = ny.zeros([1,dimm+1])
        t = float(opt)
        Curve[0,-1] = t
        for jj in range(dimm):
            for kk in range(order+1):
                Curve[0,jj] += K[kk]*Pts[kk,jj]*(1-t)**(order-kk)*t**kk
    return Curve

def pwBezier(Node, Vect, Npts):
    if Node.shape != Vect.shape:
        sys.exit('Node and Vector dimmensions should match')
    Nseg,dimm = Node.shape
    Nseg -=1
    #if hasattr(Npts, "__iter__") == False:
    #    tmp = ny.zeros([Nseg])
    #    tmp[:] = Npts
    #    Npts = tmp
    SegCurve = ny.zeros([Nseg,Npts,dimm+1])
    ControlPts = ny.zeros([4,dimm])
    for ii in range(Nseg):
        ControlPts[0,:] = Node[ii,:]
        ControlPts[1,:] = Node[ii,:]   + Vect[ii,:]
        ControlPts[2,:] = Node[ii+1,:] - Vect[ii+1,:]
        ControlPts[3,:] = Node[ii+1,:]
        SegCurve[ii,:] = BezierCurve(ControlPts,Npts)
        if ii == 0:
            Curve = SegCurve[0,:]
        else:
            tmp = SegCurve[ii,:]
            tmp[:,-1] = tmp[:,-1]+ii
            tmp = ny.delete(tmp,0,0)
            Curve = ny.vstack([Curve, tmp])
    return Curve, SegCurve

class xytCurveInterp:
    def __init__(self,curve):
        self.points = curve
        self.xt = interp.interp1d(curve[:,2],curve[:,0],'cubic')
        self.yt = interp.interp1d(curve[:,2],curve[:,1],'cubic')
    def __call__(self,t):
        xx = self.xt(t)
        yy = self.yt(t)
        point = ny.array([xx,yy,t])
        return point

def trimTe(curveXy, curveXyt, tReq):
    class Te:
        def __init__(self,curveXy,curveXyt,tReq):
            self.curveXy = interp.interp1d(curveXy[:,0],curveXy[:,1],'cubic')
            self.curveXyt = xytCurveInterp(curveXyt)
            self.tReq = tReq
        def __call__(self,t):
            pt = self.curveXyt(t)
            return self.curveXy(pt[0])-pt[1]-self.tReq
    teThick = Te(curveXy,curveXyt,tReq)
    t = bisect(teThick,curveXyt[0,2],curveXyt[-1,2])
    cutPt = teThick.curveXyt(t)
    curve1 = xyCurveSplit(curveXy,0,cutPt[0])
    curve2 = xytCurveSplit(curveXyt,cutPt[2])
    return curve1, curve2

def getDistPtPt(pt1,pt2):
        diff = pt2 - pt1
        dist = 0
        for crd in diff:
            dist +=crd**2
        return dist**.5
        
def splitCurveSimple(curveXy,curveXyt,mode):
    """
    detects closest points of two curves and splits there
    """
    distMin = 1.0e5
    for i,pt in enumerate(curveXyt):
        x,dist = getDist2(curveXy,pt[0:2])
        if dist<distMin:
            distMin = dist
            xmin = x
            imin = i
    if mode[0]==0:
        curveXy = xyCurveSplit(curveXy,0,xmin)
    else:
        curveXy = xyCurveSplit(curveXy,xmin)
    if mode[1]==0:
        curveXyt = curveXyt[:imin+1,:]
    else:
        curveXyt = curveXyt[imin:,:]
    return curveXy, curveXyt

def getCircle(center,radius,thetaStart=0, thetaEnd=180,nPts=20):
    thetaStart = math.radians(thetaStart)
    thetaEnd   = math.radians(thetaEnd)
    thetaStep  = (thetaEnd - thetaStart)/(nPts-1)
    pts = ny.zeros([nPts,2])
    theta = thetaStart
    for i in range(nPts):
        pts[i,0] = center[0] + radius*math.cos(theta)
        pts[i,1] = center[1] + radius*math.sin(theta)
        theta += thetaStep
    return pts

def normalToXyCurve(curveXy,ptX,dx=1e-4):
    tanLine = lineTanCurve(curveXy, ptX)
    return [-tanLine.direction[1],tanLine.direction[0]]

def normalToXytCurve(curveXyt,ptT,dt=1e-4):
    tU = ny.max(curveXyt[:,2])
    tL = ny.min(curveXyt[:,2])
    curveXyt = xytCurveInterp(curveXyt)
    tPlus = ptT+dt
    tMinus = ptT-dt
    if tPlus>tU:
        pt1 = curveXyt(tMinus)
        pt2 = curveXyt(ptT)
    elif tMinus<tL:
        pt1 = curveXyt(ptT)
        pt2 = curveXyt(tPlus)
    else:
        pt1 = curveXyt(tMinus)
        pt2 = curveXyt(tPlus)
    dx = pt2[0] - pt1[0]
    dy = pt2[1] - pt1[1]
    return [-dy,dx]

def getDist(curveXyt,pt):
    xL = curveXyt[0,2]
    xU = curveXyt[-1,2]
    curveXyt = xytCurveInterp(curveXyt)
    class getDistTo:
        def __init__(self,curveXyt,pt):
            self.curveXyt = curveXyt
            self.pt = pt
        def __call__(self,t):
            pt1 = self.curveXyt(t)
            x,y = self.pt
            dist = ((x-pt1[0])**2 + (y-pt1[1])**2)**0.5
            return dist
    objFcn2 = getDistTo(curveXyt,pt)
    tMinDist = fminbound(objFcn2,xL,xU)
    return tMinDist,objFcn2(tMinDist)

def getDist2(curveXy,pt):
    curve1 = interp.interp1d(curveXy[:,0],curveXy[:,1],'cubic')
    class getDistTo:
        def __init__(self,curveXy,pt):
            self.curve = curveXy
            self.pt = pt
        def __call__(self,x):
            pt1 = [x,self.curve(x)]
            return getDistPtPt(pt1,self.pt)
    objFcn3 = getDistTo(curve1,pt)
    xMinDist = fminbound(objFcn3,curveXy[0,0],curveXy[-1,0])
    return xMinDist, objFcn3(xMinDist)

def getAngle(pt1,pt2):
    dx = pt2[0] - pt1[0]
    dy = pt2[1] - pt1[1]
    if dx==0:
        return 90.0
    else:
        return math.degrees(math.atan(dy/dx))

def roundCorner(curveXy, curveXyt, rReq):
    def getOffsetPt(ptX,curveXy,rad):
        tanDir1 = normalToXyCurve(curveXy,ptX)
        curve = interp.interp1d(curveXy[:,0],curveXy[:,1],'cubic')
        x = ptX + tanDir1[0]*rad
        y = curve(ptX) + tanDir1[1]*rad
        return [x,y]
    class objFcn:
        def __init__(self,curveXy,curveXyt,rad):
            self.curveXy = curveXy
            self.curveXyt = curveXyt
            self.rad = rad
        def __call__(self,ptX):
            pt1 = getOffsetPt(ptX,self.curveXy,rReq)
            return getDist(self.curveXyt, pt1)[1]-self.rad
        def getPt(self,ptX):
            pt1 = getOffsetPt(ptX,self.curveXy,rReq)
            return getDist(self.curveXyt, pt1)[0]
            
    objFcn1 = objFcn(curveXy, curveXyt, rReq)
    xReq = bisect(objFcn1,curveXy[0,0]+.001,curveXy[-1,0]-.001)
    center = getOffsetPt(xReq,curveXy,rReq)
    curve1 = interp.interp1d(curveXy[:,0],curveXy[:,1])
    yReq = curve1(xReq)
    pt2 = objFcn1.getPt(xReq)
    curve3 = xytCurveInterp(curveXyt)
    pt3 = curve3(pt2)[0:2]
    theta1 = getAngle(center,[xReq,yReq])
    theta2 = getAngle(center,pt3)
    circle1 = getCircle(center, rReq,theta1,theta2,3)
    curveRes1 = xyCurveSplit(curveXy,[],xReq)
    curveRes2 = xytCurveSplit(curveXyt,[],pt2)
    return curveRes1, circle1, curveRes2

def normalizeVect(Vector):
    length = 0
    for VectComp in Vector:
        length += VectComp**2
    length = length**0.5
    return Vector/length

class xyCurve:
    def __init__(self,pts):
        self.curve = interp.interp1d(pts[:,0],pts[:,1],'cubic')
        self.pts = pts
        self.xU = ny.min(pts[:,0])
        self.xL = ny.max(pts[:,0])
    def tanAngle(self,x,dx=1e-3):
        dx,dy = self.tanDirection(x,dx)
        return math.atan(dy/dx)
    def tanDirection(self,x,dx=1e-3):
        dy = self.curve(x+dx) - self.curve(x-dx)
        return [2*dx, dy]
    def tan(self,x,dx=1e-4):
        dx,dy = self.tanDirection(x,dx)
        return dy/dx
    def __call__(self,x):
        return self.curve(x)
        

class linePtDir2D:
    def __init__(self, startPt, direction, length=1):
        self.direction = normalizeVect(direction)
        self.startPt = startPt
        self.endPt = startPt + length*self.direction
        self.length = length
        sp = self.startPt
        ep = self.endPt
        #Ax+By+C=0
        self.A = sp[1]-ep[1]
        self.B = ep[0]-sp[0]
        self.C = (sp[0]*ep[1]-ep[0]*sp[1])

class lineTanCurve:
    def __init__(self,curve,pointX,length=1):
        dx = 1e-4
        xL = curve[0,0]
        xU = curve[-1,0]
        if xL>xU:
            curve = ny.flipud(curve)
            xL,xU = xU,xL
        curve = interp.interp1d(curve[:,0], curve[:,1])
        ptXplus = pointX+dx
        ptXminus = pointX-dx
        if ptXplus>xU:
            dy = curve(pointX) - curve(ptXminus)
        elif ptXminus<xL:
            dy = curve(ptXplus) - curve(pointX)
        else:
            dy = (curve(ptXplus) - curve(ptXminus))/2
            

        self.direction = normalizeVect([dx,dy])
        self.startPt = [pointX, curve(pointX)]
        self.endPt = self.startPt + length*self.direction
        self.length = length
        sp = self.startPt
        ep = self.endPt
        self.A = sp[1]-ep[1]
        self.B = ep[0]-sp[0]
        self.C = (sp[0]*ep[1]-ep[0]*sp[1])

def lineIntersect(line1,line2):
    Xint = -(line1.C*line2.B - line2.C*line1.B)/(line1.A*line2.B - line2.A*line1.B)
    Yint = -(line1.A*line2.C - line2.A*line1.C)/(line1.A*line2.B - line2.A*line1.B)
    return ny.array([Xint,Yint])

def xyCurveOffset(curve,offset):
    curve2 = ny.zeros([len(curve),2])
    for i,pt in enumerate(curve):
        dir1 = normalToXyCurve(curve,pt[0])
        dir1 = normalizeVect(dir1)
        xnew = pt[0] + offset*dir1[0]
        ynew = pt[1] + offset*dir1[1]
        curve2[i] = [xnew,ynew]
    return curve2

def xytCurveOffset(curve,offset):
    curve2 = ny.zeros([len(curve),3])
    for i,pt in enumerate(curve):
        dir1 = normalToXytCurve(curve,pt[2])
        dir1 = normalizeVect(dir1)
        xnew = pt[0] + offset*dir1[0]
        ynew = pt[1] + offset*dir1[1]
        curve2[i] = [xnew,ynew,pt[2]]
    return curve2

def xyCurveSplit(curve,startPt,endPt=[]):
    NewCurve = ny.zeros([1,2])
    curveInt = interp.interp1d(curve[:,0],curve[:,1],'cubic')
    if startPt == []:
        for ii in range(curve.shape[0]):
            if curve[ii,0] < endPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
        NewCurve = ny.vstack([NewCurve,[endPt,curveInt(endPt)]])
    elif endPt == []:
        NewCurve = ny.vstack([NewCurve,[startPt,curveInt(startPt)]])
        for ii in range(curve.shape[0]):
            if curve[ii,0] > startPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
    else:
        NewCurve = ny.vstack([NewCurve,[startPt,curveInt(startPt)]])
        for ii in range(curve.shape[0]):
            if endPt > curve[ii,0] > startPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
        NewCurve = ny.vstack([NewCurve,[endPt,curveInt(endPt)]])
    NewCurve = ny.delete(NewCurve,0,0)
    return NewCurve
        
def xytCurveSplit(curve,startPt,endPt=[]):
    NewCurve = ny.zeros([1,3])
    curveInt = xytCurveInterp(curve)
    if startPt == []:
        for ii in range(curve.shape[0]):
            if curve[ii,2] < endPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
        NewCurve = ny.vstack([NewCurve,curveInt(endPt)])
    elif endPt == []:
        NewCurve = ny.vstack([NewCurve,curveInt(startPt)])
        for ii in range(curve.shape[0]):
            if curve[ii,2] > startPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
    else:
        NewCurve = ny.vstack([NewCurve,curveInt(startPt)])
        for ii in range(curve.shape[0]):
            if endPt > curve[ii,2] > startPt: NewCurve = ny.vstack([NewCurve,curve[ii,:]])
        NewCurve = ny.vstack([NewCurve,curveInt(endPt)])
    NewCurve = ny.delete(NewCurve,0,0)
    return NewCurve


def rotate2D(curve, axis, angle):
    split = False
    if curve.shape[1]>2: 
        tmp = curve[:,2:]
        curve = curve[:,0:2]
        split = True
    angle = math.radians(angle)
    rotMatrix = ny.array([[math.cos(angle), -math.sin(angle)],[math.sin(angle), math.cos(angle)]])
    rotCurve = ny.dot((curve-axis),rotMatrix) + axis
    if split:
        return ny.hstack([rotCurve,tmp])
    else:
        return rotCurve

def dist2pts(point1, point2):
    dist = 0.
    dimm = ny.min([point1.shape[0],point2.shape[0]])
    for ii in range(dimm):
        dist += (point1[ii] - point2[ii])**2
    return math.sqrt(dist)
    

def join(curves, reverse):
    N = curves.shape[0]
    if reverse[0]: 
        newCurve = curves[0][-1,:]
    else:
        newCurve = curves[0][0,:]
    for ii in range(N):
        if reverse[ii]: 
            addCurve = ny.flipud(curves[ii])
        else:
            addCurve = curves[ii]
        addCurve = ny.delete(addCurve,0,0)
        newCurve = ny.vstack([newCurve,addCurve])
    return newCurve

def adjustGap(curve,refPt,gap):
    class objFcn:
        def __init__(self,curvePts,refPt,gap):
            self.curvePts = curvePts
            self.refPt = refPt
            self.gap = gap
        def __call__(self,dy):
            curvePts = ny.copy(self.curvePts)
            curvePts[:,1] = self.curvePts[:,1] + dy
            t,dist = getDist(curvePts,self.refPt)
            return abs(dist - self.gap)
    objFcn4  = objFcn(curve,refPt,gap)
    uB = refPt[1] - ny.max(curve[:,1])
    dy = fminbound(objFcn4,-0.8,uB)
    return dy
    
def testCurve():
    node = ny.array([[0,0],[2.,2.],[4,1]])
    testVect = ny.array([[0,1.],[1.5,0],[0.5,-1]])
    
    testCurve, Curve1 = pwBezier(node,testVect,25)
    
    curve3 = xytCurveSplit(testCurve,0.2,1.7)
    
    print testCurve
    plt.plot(testCurve[:,0],testCurve[:,1],'-')
    plt.hold(True)
    plt.plot(curve3[:,0],curve3[:,1],'r-o')
    plt.grid(True)
    plt.show()

if __name__=="__main__":    
    testCurve()
    